package com.bgyrobot.coordinate.util;

import org.apache.commons.math3.linear.Array2DRowRealMatrix;
import org.apache.commons.math3.linear.LUDecomposition;
import org.apache.commons.math3.linear.RealMatrix;

/**
 * 矩阵相关工具
 * @author hesu
 */
public class MatrixUtil {
    
//    public static int DigitalScale = 12;

    /**
     * 获得单位矩阵
     */
    public static RealMatrix unitMatrix(boolean is2d) {
        if (is2d) {
            double[][] data =
              { {1.0, 0.0, 0.0},
                {0.0, 1.0, 0.0},
                {0.0, 0.0, 1.0} };
            return new Array2DRowRealMatrix(data);
        } else {
            double[][] data =
              { {1.0, 0.0, 0.0, 0.0},
                {0.0, 1.0, 0.0, 0.0},
                {0.0, 0.0, 1.0, 0.0},
                {0.0, 0.0, 0.0, 1.0} };
            return new Array2DRowRealMatrix(data);
        }
    }
    
    /**
     * 计算矩阵相乘
     */
    public static double[][] multiply(double[][] matrixLeft, double[][] matrixRight){
        RealMatrix mat1 = new Array2DRowRealMatrix(matrixLeft);
        RealMatrix mat2 = new Array2DRowRealMatrix(matrixRight);
        return mat1.multiply(mat2).getData();
    }
    
    /**
     * 计算逆矩阵
     */
    public static RealMatrix inverseMatrix(RealMatrix matrix) {
        RealMatrix mat = new LUDecomposition(matrix).getSolver().getInverse();
//        mat = scaleDigits(mat, DigitalScale);
        return mat;
    }
    
    /**
     * 计算逆矩阵
     */
    public static double[][] inverseMatrix(double[][] matrixData) {
        RealMatrix matrix = new Array2DRowRealMatrix(matrixData);
        RealMatrix outMatrix = inverseMatrix(matrix);
        return outMatrix.getData();
    }

    /**
     * 缩放矩阵
     */
    public static RealMatrix scaleMatrix(double xscale, double yscale, double zscale) {
        double[][] data = {
                {xscale, 0, 0, 0},
                {0, yscale, 0, 0},
                {0, 0, zscale, 0},
                {0, 0, 0, 1},
        };
        return new Array2DRowRealMatrix(data);
    }
    
    /**
     * 旋转矩阵
     */
    public static RealMatrix rotateMatrix(double yaw, double pitch, double roll) {
        double[][] data1 = {
                {Math.cos(yaw), Math.sin(yaw), 0, 0},
                {-Math.sin(yaw), Math.cos(yaw), 0, 0},
                {0, 0, 1, 0},
                {0, 0, 0, 1},
        };
        RealMatrix mat1 = new Array2DRowRealMatrix(data1);
        
        double[][] data2 = {
                {Math.cos(pitch), 0, -Math.sin(pitch), 0},
                {0, 1, 0, 0},
                {Math.sin(pitch), 0, Math.cos(pitch), 0},
                {0, 0, 0, 1},
        };
        RealMatrix mat2 = new Array2DRowRealMatrix(data2);

        double[][] data3 = {
                {1, 0, 0, 0},
                {0, Math.cos(roll), Math.sin(roll), 0},
                {0, -Math.sin(roll), Math.cos(roll), 0},
                {0, 0, 0, 1},
        };
        RealMatrix mat3 = new Array2DRowRealMatrix(data3);
        RealMatrix mat4 = mat1.multiply(mat2).multiply(mat3);
//        mat4 = scaleDigits(mat4, DigitalScale);
        return mat4;
    }

    /**
     * 平移矩阵
     */
    public static RealMatrix translateMatrix(double xtrans, double ytrans, double ztrans) {
        double[][] data = {
                {1, 0, 0, 0},
                {0, 1, 0, 0},
                {0, 0, 1, 0},
                {xtrans, ytrans, ztrans, 1},
        };
        return new Array2DRowRealMatrix(data);
    }
    
    /**
     * 获取初级空间转换的矩阵(Translate->Rotate(Yaw->Pitch->Roll)->Scale)
     * 输入 params 格式: [[xtrans, ytrans, ztrans], [yaw, pitch, roll], [xscale, yscale, zscale]]
     */
    public static RealMatrix primitiveMatrix(double[][] params) {
        RealMatrix mat = unitMatrix(false);
        if (params[0] != null && (params[0][0] != 0.0 || params[0][1] != 0.0 || params[0][2] != 0.0)) {
            mat = mat.multiply(translateMatrix(params[0][0], params[0][1], params[0][2]));
//            mat = scaleDigits(mat, 12);
        }
        if (params[1] != null && (params[1][0] != 0.0 || params[1][1] != 0.0 || params[1][2] != 0.0)) {
            mat = mat.multiply(rotateMatrix(params[1][0], params[1][1], params[1][2]));
//            mat = scaleDigits(mat, 12);
        }
        if (params[2] != null && (params[2][0] != 1.0 || params[2][1] != 1.0 || params[2][2] != 1.0)) {
            mat = mat.multiply(scaleMatrix(params[2][0], params[2][1], params[2][2]));
//            mat = scaleDigits(mat, 12);
        }
        return mat;
    }
    
    /**
     * 仿射变换矩阵
     * 二维仿射变换矩阵，坐标对：(3个 2d坐标对)
     * [ [[src1X, src1Y], [src2X, src2Y], [src3X, src3Y]],
     *   [[tgt1X, tgt1Y], [tgt2X, tgt2Y], [tgt2X, tgt3Y]] ]
     * 
     * 三维仿射变换矩阵，坐标对：(4个 3d坐标对)
     * [ [[src1X, src1Y, src1Z], [src2X, src2Y, src2Z], [src3X, src3Y, src3Z], [src4X, src4Y, src4Z]],
     *   [[tgt1X, tgt1Y, tgt1Z], [tgt2X, tgt2Y, tgt2Z], [tgt3X, tgt3Y, tgt3Z], [tgt4X, tgt4Y, tgt4Z]] ]
     */
    public static RealMatrix affineMatrix(double[][][] coordPairs, boolean is2d) {
        RealMatrix srcMatrix = ConvertUtil.coordArrayToMatrix(coordPairs[0]);
        RealMatrix tgtMatrix = ConvertUtil.coordArrayToMatrix(coordPairs[1]);
        RealMatrix srcMatrixI = inverseMatrix(srcMatrix);
        RealMatrix transformMatrix = srcMatrixI.multiply(tgtMatrix);
//        transformMatrix = scaleDigits(transformMatrix, DigitalScale);
        return is2d ? ConvertUtil.transformMatrix2dTo3d(transformMatrix) : transformMatrix;
    }
    
    /**
     * 执行矩阵转换 
     */
    public static double[][] transform(double[][] transformMatrix, double[][] srcCoords, boolean is2d) {
        RealMatrix srcMatrix = ConvertUtil.coordArrayToMatrix(srcCoords);
        RealMatrix transMatrix = new Array2DRowRealMatrix(transformMatrix);
        if (is2d) {
            transMatrix = ConvertUtil.transformMatrix3dTo2d(transMatrix);
        }
        RealMatrix tgtMatrix = srcMatrix.multiply(transMatrix);
//        tgtMatrix = scaleDigits(tgtMatrix, DigitalScale);
        double[][] tgtCoords = ConvertUtil.matrixToCoordArray(tgtMatrix);
        return tgtCoords;
    }
    
    /**
     * 保留N位有效数字
     * @param matrix
     * @param n
     */
    public static RealMatrix significantDigits(RealMatrix matrix, int n) {
        double[][] data = matrix.getData();
        MathUtil.significantDigits(data, n);
        return new Array2DRowRealMatrix(data);
    }
    
    /**
     * 保留N位有效数字
     * @param matrix
     * @param n
     */
    public static RealMatrix scaleDigits(RealMatrix matrix, int n) {
        double[][] data = matrix.getData();
        MathUtil.scaleDigits(data, n);
        return new Array2DRowRealMatrix(data);
    }
}
